#include "ops.h"
#include "angle.h"
#include <math.h>
int fd_ops9,fd_2;
float angle,x_pos,y_pos,tem_x,tem_y,x_pos_tem,y_pos_tem,angle1;
char data[30];
union CharFloat//单精度浮点数转换函数
{
	unsigned char str[4];
	float value;
};
float output_f(char a,char b,char c,char d)//单精度浮点数输出函数
{
     union CharFloat F;
     F.value = 0.0;
     F.str[3] = a;
     F.str[2] = b;
     F.str[1] = c;
     F.str[0] = d;
     //printf("%.4f\n",F.value);
	 return F.value;
 }

void *fun2(void *arg)//OPS——9驱动程序
{

	char s=0,flag_1=0,flag_2=0,i=0;
	char *tx1="ACT0";
	fd_2=serialOpen("/dev/ttyAMA0",115200);
	if(fd_2<0)
	{
		printf("ops-9 open fail!\r\n");
		return 0;
	}
	printf("Time:%d,OPS-9启动成功\n",millis());	
	delay(100);
	serialPuts(fd_2,tx1);
	delay(100);
    printf("serialDataAvail = %d",serialDataAvail(fd_2));
	while(1)
	{
		
		if(serialDataAvail(fd_2)>0)
		{
		s=serialGetchar(fd_2);
		
		if(s==0x0d)  
		{
			flag_1=1;
		}
		if(s==0x0a&&flag_1==1)  
		{
			flag_2=1;
			flag_1=0;
		}
		//~ printf("2");
		if(flag_2!=0)
		{
			if(s!=0x0d)
			{
				data[(unsigned char)flag_2]=(char)s;
			}
			flag_2++;
			i++;
			if(i>25)
			{
				i=0;
				angle=output_f(data[5],data[4],data[3],data[2]);         //2024.3.16陀螺仪,此句回来注释掉，并根据angle.cpp里注释修改即可
				tem_x=x_pos_tem;
				tem_y=y_pos_tem;
				x_pos_tem=output_f(data[17],data[16],data[15],data[14]);
				y_pos_tem=output_f(data[21],data[20],data[19],data[18]);
				if(fabsf(tem_x-x_pos_tem)<40)
				x_pos=x_pos_tem;
				if(fabsf(tem_y-y_pos_tem)<40)
				y_pos=y_pos_tem;
				//~ printf("x_pos=%f\n",x_pos);
				//~ printf("x_pos_tem=%f\n",x_pos_tem);
				//~ printf("y_pos=%f\n",y_pos);
				//~ printf("y_pos_tem=%f\n",y_pos_tem);
				//~ printf("angle=%f\n",angle);
				//~ printf("angle_aa=%f\n",Angle[2]);
				//~ printf("\n");
			}
		}
		if(flag_2>26)
		{
			flag_2=0;
		}
		}
		
	
	}
}
void OPS9_Init()
{
    pthread_create(&t2,NULL,fun2,NULL);//OPS-9
}
